/*
 *------------------------------------------------------------------------------
 * @File      :    ms41929.c
 * @Date      :    2021-3-20
 * @Author    :    lomboswer <lomboswer@lombotech.com>
 * @Brief     :    Source file for MDP(Media Development Platform).
 *
 * Copyright (C) 2020-2021, LomboTech Co.Ltd. All rights reserved.
 *------------------------------------------------------------------------------
 */
#include "ei_comm_camera.h"
#include "extdrv_usr.h"
#include "ei_comm_extdev.h"
#include "threadpool.h"

#ifdef __cplusplus
#if __cplusplus
extern "C" {
#endif
#endif /* __cplusplus */

#define DEV_ID 41929
#define DEV_NAME  "ms41929"

#define MOTOR_DEV  "/dev/ms419xx"

#define ZOOM_MAX  3000
#define FOCUS_MAX  3000

#define STEP_MAX  30
#define STEP_DEF  10

static VISS_DEV gaVissDev[ISP_MAX_DEV_NUM] = {-1};
static EXT_DEV_STATE_S gastExtDev[VISS_MAX_DEV_NUM] = {0};
static EI_BOOL g_sbIrInit = EI_FALSE;

typedef struct eiCTL_HEAD_S {
    EI_U32 u32Count;
    struct ext_list_head        head;
} CTL_HEAD_S;

typedef struct eiMOTOR_S {
    EI_U32  u32FocusCode;
    EI_U32  u32ZoomCode;
    EI_BOOL bBusyFlag;
    EI_U32  u32Num;
    SNS_REG_S stNow;
    threadpool_t *pPool;
    EI_U32 dev;
    pthread_mutex_t mutex;
    CTL_HEAD_S stCtlQ;
} MOTOR_S;

typedef struct eiCTL_LIST_S {
    MOTOR_ATTR_S stFocusAttr;
    MOTOR_ATTR_S stZoomAttr;
    struct ext_list_head        stNode;
} CTL_LIST_S;

EI_U8 txb[] = {0x0B, 0x80, 0x00};
EI_U8 tx0[] = {0x20, 0x01, 0x1E};
EI_U8 tx1[] = {0x21, 0x87, 0x00};
EI_U8 tx2[] = {0x22, 0x01, 0x00};
EI_U8 tx3[] = {0x23, 0xB0, 0xB0};
EI_U8 tx4[] = {0x24, 0x00, 0x12};
EI_U8 tx5[] = {0x25, 0x80, 0x02};
EI_U8 tx7[] = {0x27, 0x01, 0x00};
EI_U8 tx8[] = {0x28, 0xB0, 0xB0};
EI_U8 tx9[] = {0x29, 0x00, 0x12};
EI_U8 txa[] = {0x2A, 0x08, 0x02};
EI_U8 txc6[] = {0x2C, 0x06, 0x00};
EI_U8 txc4[] = {0x2C, 0x04, 0x00};
EI_U8 txc5[] = {0x2C, 0x05, 0x00};

static CAMERA_POWER_ACT_S s_stReset[] = {
    {MS_RESET, SNS_DOWN},
    {SNS_DELAY, 1},
    {MS_RESET, SNS_UP},
};

static CAMERA_POWER_ACT_S s_sVdFzAct[] = {
    {MS_VD_FZ, SNS_UP},
    {SNS_DELAY, 1},
    {MS_VD_FZ, SNS_DOWN},
};

static EI_S32 __EXTDEV_MotorWrite(VISS_DEV dev, int size, EI_U8 *buf)
{
    EXT_DEV_STATE_S *pastExtDevCtx = EI_NULL;

    EXT_DEV_GET_CTX(dev, pastExtDevCtx);
    CAMERA_CHECK_POINTER(pastExtDevCtx);

    return write(pastExtDevCtx->stSpiInfo.fd, buf, size);
}

static EI_VOID __EXTDEV_MsbToLsb(EI_U8 *buf)
{
    EI_U8 src = 0;
    EI_U8 tmp = 0;
    int i = 0;

    src = *buf;
    for (i = 0; i < 8; i++) {
        if (src & (1 << i))
            tmp |= (1 << (7 - i));
    }

    *buf = tmp;
}

static EI_VOID __EXTDEV_FreeCtrlQ(MOTOR_S *pMotor)
{
    struct ext_list_head *pstNode;
    CTL_LIST_S *pCtl = EI_NULL;

    while(!ext_list_empty(&pMotor->stCtlQ.head)) {
        pstNode = pMotor->stCtlQ.head.next;
        pMotor->stCtlQ.u32Count--;
        ext_list_del(pstNode);
        pCtl = ext_container_of(pstNode, CTL_LIST_S, stNode);
        EXT_DEV_FREE(pCtl);
    }
}

static EI_VOID __EXTDEV_Process(void *arg)
{
    MOTOR_S *pstMotor = (MOTOR_S *)arg;
    CTL_LIST_S *pCtl = EI_NULL;
    struct ext_list_head *pstNode;
    EI_BOOL bSkip = EI_FALSE;
    pstMotor->bBusyFlag = EI_TRUE;
    EI_U8 u8Focus[3] = {0};
    EI_U8 u8Zoom[3] = {0};
    EI_U8 u8FocusSpeed[3] = {0};
    EI_U8 u8ZoomSpeed[3] = {0};
    EI_U16 u16Speed = 0;
    EI_U8 u8VDHz = 60;//PPS=800, so max step 8Unit per 10ms

    u8Focus[0] = tx4[0];
    u8FocusSpeed[0] = tx5[0];
    u8Zoom[0] = tx9[0];
    u8ZoomSpeed[0] = txa[0];

    while(1) {
        pthread_mutex_lock(&pstMotor->mutex);
        if (!ext_list_empty(&pstMotor->stCtlQ.head)) {
            pstNode = pstMotor->stCtlQ.head.next;
            pstMotor->stCtlQ.u32Count--;
            ext_list_del(pstNode);
            pCtl = ext_container_of(pstNode, CTL_LIST_S, stNode);
        } else {
            pthread_mutex_unlock(&pstMotor->mutex);
            u8Focus[1] = 0x00;
            u8Focus[2] = 0x12;
            u8Zoom[1] = 0x00;
            u8Zoom[2] = 0x12;
            bSkip = EI_TRUE;
            goto SKIP;
        }
        pthread_mutex_unlock(&pstMotor->mutex);

        if (pCtl->stFocusAttr.u32Unit) {
            u8Focus[1] = (pCtl->stFocusAttr.u32Unit > STEP_MAX ? STEP_MAX : pCtl->stFocusAttr.u32Unit)*8;
            u8Focus[2] = 0x14;
            if (pCtl->stFocusAttr.u32Dir) {
                u8Focus[2] |= 0x01;
            }
            u16Speed = 27000000 / u8VDHz / (u8Focus[1] * 24);
            u8FocusSpeed[1] = u16Speed & 0x00ff;
            u8FocusSpeed[2] = u16Speed >> 8;
        } else {
            u8Focus[1] = 0x00;
            u8Focus[2] = 0x12;
        }

        if (pCtl->stZoomAttr.u32Unit) {
            u8Zoom[1] = (pCtl->stZoomAttr.u32Unit > STEP_MAX ? STEP_MAX : pCtl->stZoomAttr.u32Unit)*8;
            u8Zoom[2] = 0x14;
            if (pCtl->stZoomAttr.u32Dir) {
                u8Zoom[2] |= 0x01;
            }
            u16Speed = 27000000 / u8VDHz / (u8Zoom[1] * 24);
            u8ZoomSpeed[1] = u16Speed & 0x00ff;
            u8ZoomSpeed[2] = u16Speed >> 8;
        } else {
            u8Zoom[1] = 0x00;
            u8Zoom[2] = 0x12;
        }

SKIP:
        __EXTDEV_MsbToLsb(&u8Focus[1]);
        __EXTDEV_MsbToLsb(&u8Zoom[1]);

        __EXTDEV_MsbToLsb(&u8Focus[2]);
        __EXTDEV_MsbToLsb(&u8Zoom[2]);

        __EXTDEV_MsbToLsb(&u8FocusSpeed[1]);
        __EXTDEV_MsbToLsb(&u8FocusSpeed[2]);
        __EXTDEV_MsbToLsb(&u8ZoomSpeed[1]);
        __EXTDEV_MsbToLsb(&u8ZoomSpeed[2]);

        pthread_mutex_lock(&pstMotor->mutex);
        __EXTDEV_MotorWrite(pstMotor->dev,3, u8Focus);
        __EXTDEV_MotorWrite(pstMotor->dev,3, u8Zoom);
        __EXTDEV_MotorWrite(pstMotor->dev,3, u8FocusSpeed);
        __EXTDEV_MotorWrite(pstMotor->dev,3, u8ZoomSpeed);
        VISS_SetPinState(pstMotor->dev, s_sVdFzAct, ARRARSIZE(s_sVdFzAct));
        ei_msleep(EI_ALIGN_UP(1000 / u8VDHz, 10));/* (INTCTXX[15:0]*PSUMXX[7:0]*24)/27M */
        pthread_mutex_unlock(&pstMotor->mutex);

        EXT_DEV_FREE(pCtl);

        if (bSkip) {
            pthread_mutex_lock(&pstMotor->mutex);
            if (ext_list_empty(&pstMotor->stCtlQ.head)) {
                pthread_mutex_unlock(&pstMotor->mutex);
                break;
            }
            bSkip = EI_FALSE;
            pthread_mutex_unlock(&pstMotor->mutex);
        }
    }

    pstMotor->bBusyFlag = EI_FALSE;

    return;
}

static EI_S32 __EXTDEV_AddCtlAttr(VISS_DEV dev, MOTOR_ATTR_S *pMotorAttr)
{
    EI_S32 s32Ret = EI_SUCCESS;
    EXT_DEV_STATE_S *pastExtDevCtx = EI_NULL;
    MOTOR_S *pMotor;
    CTL_LIST_S *pCtlAttr = EI_NULL;

    EXT_DEV_GET_CTX(dev, pastExtDevCtx);

    pCtlAttr = (CTL_LIST_S *)malloc(sizeof(CTL_LIST_S));
    CAMERA_CHECK_POINTER(pCtlAttr);

    pMotor = (MOTOR_S *)pastExtDevCtx->pvPriv;

    pCtlAttr->stZoomAttr = pMotorAttr[0];
    pCtlAttr->stFocusAttr = pMotorAttr[1];
    ext_list_add_tail(&pCtlAttr->stNode, &pMotor->stCtlQ.head);
    pMotor->stCtlQ.u32Count++;

    return s32Ret;
}

static EI_VOID __EXTDEV_MsbToLsbs(EI_U32 size, EI_U8 *buf)
{
    EI_U32 i;

    for (i = 0; i < size; i++) {
        __EXTDEV_MsbToLsb(&buf[i]);
    }
}

static EI_S32 __EXTDEV_SwitchIrCut(VISS_DEV dev, IR_STATE_E eState)
{
    EI_S32 s32Ret = EI_SUCCESS;

    if(eState) {
        s32Ret = __EXTDEV_MotorWrite(dev,3, txc5);
        CAMERA_CHECK_SUCCESS(s32Ret);
        s32Ret = VISS_SetPinState(dev, s_sVdFzAct, ARRARSIZE(s_sVdFzAct));
        CAMERA_CHECK_SUCCESS(s32Ret);

        ei_msleep(50);

        s32Ret = __EXTDEV_MotorWrite(dev,3, txc4);
        CAMERA_CHECK_SUCCESS(s32Ret);
        s32Ret = VISS_SetPinState(dev, s_sVdFzAct, ARRARSIZE(s_sVdFzAct));
        CAMERA_CHECK_SUCCESS(s32Ret);
    } else {
        s32Ret = __EXTDEV_MotorWrite(dev,3, txc6);
        CAMERA_CHECK_SUCCESS(s32Ret);
        s32Ret = VISS_SetPinState(dev, s_sVdFzAct, ARRARSIZE(s_sVdFzAct));
        CAMERA_CHECK_SUCCESS(s32Ret);

        ei_msleep(50);

        s32Ret = __EXTDEV_MotorWrite(dev,3, txc4);
        CAMERA_CHECK_SUCCESS(s32Ret);
        s32Ret = VISS_SetPinState(dev, s_sVdFzAct, ARRARSIZE(s_sVdFzAct));
        CAMERA_CHECK_SUCCESS(s32Ret);
    }

    return s32Ret;
}

static EI_S32 EXTDEV_MotorInit(VISS_DEV dev)
{
    EI_S32 s32Ret = EI_SUCCESS;
    EXT_DEV_STATE_S *pastExtDevCtx = EI_NULL;
    MOTOR_S *pMotor;

    EXT_DEV_GET_CTX(dev, pastExtDevCtx);
    CAMERA_CHECK_POINTER(pastExtDevCtx);

    pMotor = (MOTOR_S *)pastExtDevCtx->pvPriv;

    pthread_mutex_lock(&pMotor->mutex);

    s32Ret |= VISS_SetPinState(dev, s_stReset, ARRARSIZE(s_stReset));

    if (txb[0] == 0x0b) {
        __EXTDEV_MsbToLsbs(3, txb);
        __EXTDEV_MsbToLsbs(3, tx0);
        __EXTDEV_MsbToLsbs(3, tx1);
        __EXTDEV_MsbToLsbs(3, tx2);
        __EXTDEV_MsbToLsbs(3, tx3);
        __EXTDEV_MsbToLsbs(3, tx4);
        __EXTDEV_MsbToLsbs(3, tx5);
        __EXTDEV_MsbToLsbs(3, tx7);
        __EXTDEV_MsbToLsbs(3, tx8);
        __EXTDEV_MsbToLsbs(3, tx9);
        __EXTDEV_MsbToLsbs(3, txa);
        __EXTDEV_MsbToLsbs(3, txc6);
        __EXTDEV_MsbToLsbs(3, txc4);
        __EXTDEV_MsbToLsbs(3, txc5);
    }

    s32Ret |= __EXTDEV_MotorWrite(dev, 3, txb);
    s32Ret |= __EXTDEV_MotorWrite(dev, 3, tx0);
    s32Ret |= __EXTDEV_MotorWrite(dev, 3, tx1);
    s32Ret |= __EXTDEV_MotorWrite(dev, 3, tx2);
    s32Ret |= __EXTDEV_MotorWrite(dev, 3, tx3);
    s32Ret |= __EXTDEV_MotorWrite(dev, 3, tx4);
    s32Ret |= __EXTDEV_MotorWrite(dev, 3, tx7);
    s32Ret |= __EXTDEV_MotorWrite(dev, 3, tx8);
    s32Ret |= __EXTDEV_MotorWrite(dev, 3, tx9);
    s32Ret |= __EXTDEV_MotorWrite(dev, 3, tx5);
    s32Ret |= __EXTDEV_MotorWrite(dev, 3, txa);
    s32Ret |= VISS_SetPinState(dev, s_sVdFzAct, ARRARSIZE(s_sVdFzAct));

    if (g_sbIrInit == EI_FALSE) {
        __EXTDEV_SwitchIrCut(dev, IR_CLOSE);
        __EXTDEV_SwitchIrCut(dev, IR_OPEN);
        pastExtDevCtx->eIr = IR_OPEN;
        g_sbIrInit = EI_TRUE;
    }

    pthread_mutex_unlock(&pMotor->mutex);

    if (pastExtDevCtx->enExtDevType == MS41929_MOTOR) {
        s32Ret = threadpool_add(pMotor->pPool, &__EXTDEV_Process, pMotor, 0);
        CAMERA_CHECK_SUCCESS(s32Ret);
    }

    extprintf("--%s Init--\n", DEV_NAME);

    return s32Ret;
}

static void EXTDEV_MotorExit(VISS_DEV dev)
{
    extprintf("--%s Exit--\n", DEV_NAME);
}

static EI_S32 EXTDEV_SetMotorCode(VISS_DEV VissDev, EI_U32 u32ZoomCode, EI_U32 u32FocusCode)
{
    EI_S32 s32Ret = EI_SUCCESS;
    EXT_DEV_STATE_S *pastExtDevCtx = EI_NULL;
    MOTOR_S *pMotor;
    MOTOR_ATTR_S aMotorAttr[2] = {0};
    EI_U32 u32ZUnit;
    EI_U32 u32FUnit;

    EXT_DEV_GET_CTX(VissDev, pastExtDevCtx);
    CAMERA_CHECK_POINTER(pastExtDevCtx);
    CAMERA_CHECK_POINTER(pastExtDevCtx->pvPriv);

    if (pastExtDevCtx->enExtDevType != MS41929_MOTOR)
        return EI_ERR_VISS_NOT_SUPPORT;

    if (u32FocusCode > FOCUS_MAX) {
        extprintf("FocusCodeMax:%d, set val %d\n", FOCUS_MAX, u32FocusCode);
        return EI_ERR_VISS_ILLEGAL_PARAM;
    }

    if (u32ZoomCode > ZOOM_MAX) {
        extprintf("ZoomCodeMax:%d, set val %d\n", ZOOM_MAX, u32ZoomCode);
        return EI_ERR_VISS_ILLEGAL_PARAM;
    }

    pMotor = (MOTOR_S *)pastExtDevCtx->pvPriv;

    pthread_mutex_lock(&pMotor->mutex);

    if ((u32FocusCode != pMotor->u32FocusCode) || (u32ZoomCode != pMotor->u32ZoomCode)) {

        if (u32FocusCode == pMotor->u32FocusCode) {
            u32FUnit = 0;
        } else {
            aMotorAttr[1].u32Dir = (u32FocusCode > pMotor->u32FocusCode) ? 0 : 1;
            u32FUnit = (u32FocusCode > pMotor->u32FocusCode) ?
            (u32FocusCode - pMotor->u32FocusCode) : (pMotor->u32FocusCode - u32FocusCode);
        }

        if (u32ZoomCode == pMotor->u32ZoomCode) {
            u32ZUnit = 0;
        } else {
            aMotorAttr[0].u32Dir = (u32ZoomCode > pMotor->u32ZoomCode) ? 0 : 1;
            u32ZUnit = (u32ZoomCode > pMotor->u32ZoomCode) ?
            (u32ZoomCode - pMotor->u32ZoomCode) : (pMotor->u32ZoomCode - u32ZoomCode);
        }

        if (u32FUnit >= u32ZUnit) {
            while (u32FUnit >= STEP_DEF) {
                aMotorAttr[1].u32Unit = STEP_DEF;
                u32FUnit -= STEP_DEF;

                if (u32ZUnit >= STEP_DEF) {
                    aMotorAttr[0].u32Unit = STEP_DEF;
                    u32ZUnit -= STEP_DEF;
                }

                __EXTDEV_AddCtlAttr(VissDev, aMotorAttr);
            }

            if (u32FUnit || u32ZUnit) {
                aMotorAttr[1].u32Unit = u32FUnit;
                aMotorAttr[0].u32Unit = u32ZUnit;
                __EXTDEV_AddCtlAttr(VissDev, aMotorAttr);
            }
        } else {
            while (u32ZUnit >= STEP_DEF) {
                aMotorAttr[0].u32Unit = STEP_DEF;
                u32ZUnit -= STEP_DEF;

                if (u32FUnit >= STEP_DEF) {
                    aMotorAttr[1].u32Unit = STEP_DEF;
                    u32FUnit -= STEP_DEF;
                }

                __EXTDEV_AddCtlAttr(VissDev, aMotorAttr);
            }

            if (u32ZUnit || u32FUnit) {
                aMotorAttr[1].u32Unit = u32FUnit;
                aMotorAttr[0].u32Unit = u32ZUnit;
                __EXTDEV_AddCtlAttr(VissDev, aMotorAttr);
            }
        }

        pMotor->u32FocusCode = u32FocusCode;
        pMotor->u32ZoomCode = u32ZoomCode;
    }

    pthread_mutex_unlock(&pMotor->mutex);

    s32Ret = threadpool_add(pMotor->pPool, &__EXTDEV_Process, pMotor, 0);
    CAMERA_CHECK_SUCCESS(s32Ret);

    return s32Ret;
}

static EI_S32 EXTDEV_GetMotorCode(VISS_DEV VissDev, EI_U32 *pu32ZoomCode, EI_U32 *pu32FocusCode)
{
    EI_S32 s32Ret = EI_SUCCESS;
    EXT_DEV_STATE_S *pastExtDevCtx = EI_NULL;
    MOTOR_S *pMotor;

    EXT_DEV_GET_CTX(VissDev, pastExtDevCtx);

    CAMERA_CHECK_POINTER(pastExtDevCtx);
    CAMERA_CHECK_POINTER(pastExtDevCtx->pvPriv);

    pMotor = (MOTOR_S *)pastExtDevCtx->pvPriv;

    pthread_mutex_lock(&pMotor->mutex);
    *pu32FocusCode = pMotor->u32FocusCode;
    *pu32ZoomCode = pMotor->u32ZoomCode;
    pthread_mutex_unlock(&pMotor->mutex);

    return s32Ret;
}

static EI_S32 EXTDEV_SetExtDevAttr(VISS_DEV dev,const CTL_ATTR_S *pstCtlAttr)
{
    EI_S32 s32Ret = EI_SUCCESS;
    EXT_DEV_STATE_S *pastExtDevCtx = EI_NULL;
    MOTOR_S *pMotor;

    EXT_DEV_GET_CTX(dev, pastExtDevCtx);
    CAMERA_CHECK_POINTER(pastExtDevCtx);
    CAMERA_CHECK_POINTER(pastExtDevCtx->pvPriv);

    pMotor = (MOTOR_S *)pastExtDevCtx->pvPriv;

    switch (pstCtlAttr->enType) {
        case IR_CUT:
            pthread_mutex_lock(&pMotor->mutex);
            pastExtDevCtx->eIr = pstCtlAttr->unAttr.stIrCut.eState;
            __EXTDEV_SwitchIrCut(dev, pstCtlAttr->unAttr.stIrCut.eState);
            pthread_mutex_unlock(&pMotor->mutex);
            break;
        case MOTOR:
            s32Ret = EXTDEV_SetMotorCode(dev, pstCtlAttr->unAttr.stMotorCode.u32ZoomCode,
                pstCtlAttr->unAttr.stMotorCode.u32FocusCode);
            if (s32Ret != EI_SUCCESS) {
                PRT_VISS_ERR("EXTDEV_SetMotorCode %x\n", s32Ret);
            }
            break;
        default:
            PRT_VISS_ERR("%s  type err\n", DEV_NAME);
            return EI_FAILURE;
    }

    return s32Ret;
}

static EI_S32 EXTDEV_GetExtDevAttr(VISS_DEV dev, CTL_ATTR_S *pstCtlAttr)
{
    EI_S32 s32Ret = EI_SUCCESS;
    EXT_DEV_STATE_S *pastExtDevCtx = EI_NULL;
    MOTOR_S *pMotor;

    EXT_DEV_GET_CTX(dev, pastExtDevCtx);
    CAMERA_CHECK_POINTER(pastExtDevCtx);
    CAMERA_CHECK_POINTER(pastExtDevCtx->pvPriv);

    pMotor = (MOTOR_S *)pastExtDevCtx->pvPriv;

    switch (pstCtlAttr->enType) {
        case IR_CUT:
            pthread_mutex_lock(&pMotor->mutex);
            pstCtlAttr->unAttr.stIrCut.eState = pastExtDevCtx->eIr;
            pthread_mutex_unlock(&pMotor->mutex);
            break;
        case MOTOR:
            s32Ret = EXTDEV_GetMotorCode(dev, &pstCtlAttr->unAttr.stMotorCode.u32ZoomCode,
                &pstCtlAttr->unAttr.stMotorCode.u32FocusCode);
            if (s32Ret != EI_SUCCESS) {
                PRT_VISS_ERR("EXTDEV_GetMotorCode %x\n", s32Ret);
            }
            break;
        default:
            PRT_VISS_ERR("%s  type err\n", DEV_NAME);
            return EI_FAILURE;
    }

    return s32Ret;
}

static EI_S32 EXTDEV_SetZoomCode(ISP_DEV dev, EI_U32 u32ZoomCode)
{
    EI_S32 s32Ret = EI_SUCCESS;
    EXT_DEV_STATE_S *pastExtDevCtx = EI_NULL;
    MOTOR_S *pMotor;
    VISS_DEV VissDev;
    MOTOR_ATTR_S aMotorAttr[2] = {0};
    EI_U32 i;
    EI_U32 u32Unit;

    SNS_GET_ISP_TO_VISS(dev, VissDev);

    EXT_DEV_GET_CTX(VissDev, pastExtDevCtx);
    CAMERA_CHECK_POINTER(pastExtDevCtx);
    CAMERA_CHECK_POINTER(pastExtDevCtx->pvPriv);

    if (pastExtDevCtx->enExtDevType != MS41929_MOTOR)
        return EI_ERR_VISS_NOT_SUPPORT;

    if (u32ZoomCode > ZOOM_MAX) {
        extprintf("ZoomCodeMax:%d, set val %d\n", ZOOM_MAX, u32ZoomCode);
        return EI_ERR_VISS_ILLEGAL_PARAM;
    }

    pMotor = (MOTOR_S *)pastExtDevCtx->pvPriv;

    pthread_mutex_lock(&pMotor->mutex);

    if (u32ZoomCode != pMotor->u32ZoomCode) {
        aMotorAttr[0].u32Dir = (u32ZoomCode > pMotor->u32ZoomCode) ? 0 : 1;
        u32Unit = (u32ZoomCode > pMotor->u32ZoomCode) ?
        (u32ZoomCode - pMotor->u32ZoomCode) : (pMotor->u32ZoomCode - u32ZoomCode);

        for (i = 0; i < (u32Unit / STEP_DEF); i++) {
            aMotorAttr[0].u32Unit = STEP_DEF;
            __EXTDEV_AddCtlAttr(VissDev, aMotorAttr);
        }

        if (u32Unit % STEP_DEF) {
            aMotorAttr[0].u32Unit = u32Unit % STEP_DEF;
            __EXTDEV_AddCtlAttr(VissDev, aMotorAttr);
        }

        pMotor->u32ZoomCode = u32ZoomCode;
    }

    pthread_mutex_unlock(&pMotor->mutex);

    s32Ret = threadpool_add(pMotor->pPool, &__EXTDEV_Process, pMotor, 0);
    CAMERA_CHECK_SUCCESS(s32Ret);

    return s32Ret;
}

static EI_S32 EXTDEV_GetZoomCode(ISP_DEV dev, EI_U32 *pu32ZoomCode)
{
    EI_S32 s32Ret = EI_SUCCESS;
    EXT_DEV_STATE_S *pastExtDevCtx = EI_NULL;
    MOTOR_S *pMotor;
    VISS_DEV VissDev;
    SNS_GET_ISP_TO_VISS(dev,VissDev);

    EXT_DEV_GET_CTX(VissDev, pastExtDevCtx);

    CAMERA_CHECK_POINTER(pastExtDevCtx);
    CAMERA_CHECK_POINTER(pastExtDevCtx->pvPriv);

    pMotor = (MOTOR_S *)pastExtDevCtx->pvPriv;

    pthread_mutex_lock(&pMotor->mutex);
    *pu32ZoomCode = pMotor->u32ZoomCode;
    pthread_mutex_unlock(&pMotor->mutex);

    return s32Ret;
}

static EI_S32 EXTDEV_SetFocusCode(ISP_DEV dev, EI_U32 u32FocusCode)
{
    EI_S32 s32Ret = EI_SUCCESS;
    EXT_DEV_STATE_S *pastExtDevCtx = EI_NULL;
    MOTOR_S *pMotor;
    VISS_DEV VissDev;
    MOTOR_ATTR_S aMotorAttr[2] = {0};
    EI_U32 i;
    EI_U32 u32Unit;

    if (u32FocusCode > FOCUS_MAX) {
        extprintf("FocusCodeMax:%d, set val %d\n", FOCUS_MAX, u32FocusCode);
        return EI_ERR_VISS_ILLEGAL_PARAM;
    }

    SNS_GET_ISP_TO_VISS(dev,VissDev);

    EXT_DEV_GET_CTX(VissDev, pastExtDevCtx);
    CAMERA_CHECK_POINTER(pastExtDevCtx);
    CAMERA_CHECK_POINTER(pastExtDevCtx->pvPriv);

    if (pastExtDevCtx->enExtDevType != MS41929_MOTOR)
        return EI_ERR_VISS_NOT_SUPPORT;

    pMotor = (MOTOR_S *)pastExtDevCtx->pvPriv;

    pthread_mutex_lock(&pMotor->mutex);

    if (u32FocusCode != pMotor->u32FocusCode) {
        aMotorAttr[1].u32Dir = (u32FocusCode > pMotor->u32FocusCode) ? 0 : 1;
        u32Unit = (u32FocusCode > pMotor->u32FocusCode) ?
        (u32FocusCode - pMotor->u32FocusCode) : (pMotor->u32FocusCode - u32FocusCode);

        for (i = 0; i < (u32Unit / STEP_DEF); i++) {
            aMotorAttr[1].u32Unit = STEP_DEF;
            __EXTDEV_AddCtlAttr(VissDev, aMotorAttr);
        }

        if (u32Unit % STEP_DEF) {
            aMotorAttr[1].u32Unit = u32Unit % STEP_DEF;
            __EXTDEV_AddCtlAttr(VissDev, aMotorAttr);
        }
        pMotor->u32FocusCode = u32FocusCode;
    }

    pthread_mutex_unlock(&pMotor->mutex);

    s32Ret = threadpool_add(pMotor->pPool, &__EXTDEV_Process, pMotor, 0);
    CAMERA_CHECK_SUCCESS(s32Ret);

    return s32Ret;
}

static EI_S32 EXTDEV_GetFocusCode(ISP_DEV dev, EI_U32 *pu32FocusCode)
{
    EI_S32 s32Ret = EI_SUCCESS;
    EXT_DEV_STATE_S *pastExtDevCtx = EI_NULL;
    MOTOR_S *pMotor;
    VISS_DEV VissDev;

    SNS_GET_ISP_TO_VISS(dev,VissDev);

    EXT_DEV_GET_CTX(VissDev, pastExtDevCtx);

    CAMERA_CHECK_POINTER(pastExtDevCtx);
    CAMERA_CHECK_POINTER(pastExtDevCtx->pvPriv);

    pMotor = (MOTOR_S *)pastExtDevCtx->pvPriv;

    pthread_mutex_lock(&pMotor->mutex);
    *pu32FocusCode = pMotor->u32FocusCode;
    pthread_mutex_unlock(&pMotor->mutex);

    return s32Ret;
}

static EI_S32 EXTDEV_IspSetMotorCode(ISP_DEV dev, EI_U32 u32ZoomCode, EI_U32 u32FocusCode)
{

    VISS_DEV VissDev;

    SNS_GET_ISP_TO_VISS(dev,VissDev);

    return EXTDEV_SetMotorCode(VissDev, u32ZoomCode, u32FocusCode);
}

static EI_S32 EXTDEV_IspGetMotorCode(ISP_DEV dev, EI_U32 *pu32ZoomCode, EI_U32 *pu32FocusCode)
{

    VISS_DEV VissDev;

    SNS_GET_ISP_TO_VISS(dev,VissDev);

    return EXTDEV_GetMotorCode(VissDev, pu32ZoomCode, pu32FocusCode);
}


static EI_S32 EXTDEV_MotorReset(ISP_DEV dev, EI_BOOL bReset)
{
    VISS_DEV VissDev;

    SNS_GET_ISP_TO_VISS(dev,VissDev);

    return EXTDEV_SetMotorCode(VissDev, ZOOM_MAX, FOCUS_MAX);
}

static EI_BOOL EXTDEV_MotorBusyReq(ISP_DEV dev)
{
    EXT_DEV_STATE_S *pastExtDevCtx = EI_NULL;
    MOTOR_S *pMotor;
    VISS_DEV VissDev;

    SNS_GET_ISP_TO_VISS(dev,VissDev);

    EXT_DEV_GET_CTX(VissDev, pastExtDevCtx);

    CAMERA_CHECK_POINTER(pastExtDevCtx);
    CAMERA_CHECK_POINTER(pastExtDevCtx->pvPriv);

    pMotor = (MOTOR_S *)pastExtDevCtx->pvPriv;

    return pMotor->bBusyFlag;
}

static int __EXTDEV_SetSpiInfo(VISS_DEV dev)
{
    EXT_DEV_STATE_S *pastExtDevCtx = EI_NULL;

    EXT_DEV_GET_CTX(dev, pastExtDevCtx);
    CAMERA_CHECK_POINTER(pastExtDevCtx);

    if (pastExtDevCtx->stSpiInfo.fd > 0) {
        return EI_SUCCESS;
    }

    return EI_SUCCESS;
}

/* The following functions can be modified as needed */

static int __EXTDEV_SpiInit(VISS_DEV dev)
{
    char acDevFile[16] = {0};
    EXT_DEV_STATE_S *pastExtDevCtx = EI_NULL;
    //EI_U8 u8DevNum;

    EXT_DEV_GET_CTX(dev, pastExtDevCtx);
    EXT_DEV_CHECK_POINTER(pastExtDevCtx);

    __EXTDEV_SetSpiInfo(dev);

    //ret = CAMERA_I2C_Reg_Ops(&pstSnsState->stI2cInfo);

    //u8DevNum = pastExtDevCtx->stSpiInfo.u16SpiBusNum;

    snprintf(acDevFile, sizeof(acDevFile),  MOTOR_DEV);

    pastExtDevCtx->stSpiInfo.fd = open(acDevFile, O_RDWR);

    if (pastExtDevCtx->stSpiInfo.fd < 0) {
        EI_TRACE_VISS(EI_DBG_ERR, "Open %s error!\n", MOTOR_DEV);
        return EI_FAILURE;
    }

    PRT_VISS_INFO("!!!!!! open %s !!!!!! \r\n", MOTOR_DEV);

    return EI_SUCCESS;
}

static int __EXTDEV_SpiExit(VISS_DEV dev)
{
    EXT_DEV_STATE_S *pastExtDevCtx = EI_NULL;

    EXT_DEV_GET_CTX(dev, pastExtDevCtx);
    EXT_DEV_CHECK_POINTER(pastExtDevCtx);

    if (pastExtDevCtx->stSpiInfo.fd > 0) {
        close(pastExtDevCtx->stSpiInfo.fd);
        pastExtDevCtx->stSpiInfo.fd = -1;
        return EI_SUCCESS;
    }

    return EI_FAILURE;
}

static EI_S32 __EXTDEV_InitExpFunction(EXT_DEV_FUNC_S *pstExtDevFunc)
{
    EXT_DEV_CHECK_POINTER(pstExtDevFunc);

    memset(pstExtDevFunc, 0, sizeof(EXT_DEV_FUNC_S));

    pstExtDevFunc->pfnExtDevInit     = EXTDEV_MotorInit;
    pstExtDevFunc->pfnExtDevSetAttr   = EXTDEV_SetExtDevAttr;
    pstExtDevFunc->pfnExtDevGetAttr   = EXTDEV_GetExtDevAttr;
    pstExtDevFunc->pfnExtDevExit     = EXTDEV_MotorExit;

    return EI_SUCCESS;
}

/****************************************************************************
 * callback structure                                                       *
 ****************************************************************************/

static EI_S32 __EXTDEV_CtxInit(VISS_DEV dev, EXT_DEV_TYPE_E eType)
{
    EXT_DEV_STATE_S *pastExtDevCtx = EI_NULL;
    MOTOR_S *pMotor = EI_NULL;

    EXT_DEV_GET_CTX(dev, pastExtDevCtx);

    memset(pastExtDevCtx, 0, sizeof(EXT_DEV_STATE_S));

    pastExtDevCtx->pvPriv = malloc(sizeof(MOTOR_S));
    if (pastExtDevCtx->pvPriv == EI_NULL) {
        PRT_VISS_ERR("[%d] pvPriv malloc memory failed!\n", dev);
        return -EN_ERR_NOMEM;
    }

    memset(pastExtDevCtx->pvPriv, 0, sizeof(MOTOR_S));

    pMotor = (MOTOR_S *)pastExtDevCtx->pvPriv;

    EXT_INIT_LIST_HEAD(&pMotor->stCtlQ.head);

    pthread_mutex_init(&pMotor->mutex, NULL);
    pMotor->dev = dev;

    pastExtDevCtx->enExtDevType      = eType;
    pastExtDevCtx->pcName            = DEV_NAME;
    pastExtDevCtx->pstSetAct[0]      = s_stReset;
    pastExtDevCtx->u8SetActs[0]      = ARRARSIZE(s_stReset);

    pastExtDevCtx->pstSetAct[1]      = s_sVdFzAct;
    pastExtDevCtx->u8SetActs[1]      = ARRARSIZE(s_sVdFzAct);

    pMotor->u32FocusCode = 0;
    pMotor->u32ZoomCode = 0;

    if (pastExtDevCtx->enExtDevType == MS41929_MOTOR) {
        pMotor->pPool = threadpool_create(1, 5, DEV_NAME);
        EXT_DEV_CHECK_POINTER(pMotor->pPool);
    }

    return EI_SUCCESS;
}

static EI_VOID __EXTDEV_CtxExit(VISS_DEV dev)
{
    EXT_DEV_STATE_S *pastExtDevCtx = EI_NULL;
    MOTOR_S *pMotor;

    EXT_DEV_GET_CTX(dev, pastExtDevCtx);

    pMotor = (MOTOR_S *)pastExtDevCtx->pvPriv;

    pthread_mutex_lock(&pMotor->mutex);
    __EXTDEV_FreeCtrlQ(pMotor);
    pthread_mutex_unlock(&pMotor->mutex);

    if (pastExtDevCtx->enExtDevType == MS41929_MOTOR) {
        if (threadpool_destroy(pMotor->pPool, 0)) {
            extprintf("threadpool_destroy err\n");
        }
    }

    pthread_mutex_destroy(&pMotor->mutex);

    EXT_DEV_FREE(pMotor);
}

static EI_S32 EXTDEV_RegisterVissCallback(VISS_DEV dev, EXT_DEV_TYPE_E eType)
{
    EI_S32 s32Ret;
    EXT_INFO_S stExtInfo;
    EXT_DEV_STATE_S *pastExtDevCtx = EI_NULL;

    s32Ret = __EXTDEV_CtxInit(dev, eType);
    if (EI_SUCCESS != s32Ret) {
        return EI_FAILURE;
    }

    s32Ret = __EXTDEV_SpiInit(dev);
    if (EI_SUCCESS != s32Ret) {
        return EI_FAILURE;
    }

    EXT_DEV_GET_CTX(dev, pastExtDevCtx);

    stExtInfo.pstExtDev = pastExtDevCtx;

    s32Ret  = __EXTDEV_InitExpFunction(&stExtInfo.stExtDevFun);
    s32Ret |= VISS_ExtDevRegCallBack(dev, &stExtInfo);
    if (EI_SUCCESS != s32Ret) {
        PRT_VISS_ERR("%s ext dev register callback function failed!\n", DEV_NAME);
        return s32Ret;
    }

    return EI_SUCCESS;
}

static EI_S32 EXTDEV_UnregisterVissCallback(VISS_DEV dev)
{
    EI_S32 s32Ret;

    s32Ret = VISS_ExtDevUnRegCallBack(dev);
    if (EI_SUCCESS != s32Ret) {
        PRT_VISS_ERR("%s ext dev unregister callback function failed!\n", DEV_NAME);
        return s32Ret;
    }
    __EXTDEV_SpiExit(dev);

    __EXTDEV_CtxExit(dev);

    return EI_SUCCESS;
}
static EI_S32 EXTDEV_InitAfExpFunction(AF_DRIVER_FUNC_S *pstExpFuncs)
{
    CAMERA_CHECK_POINTER(pstExpFuncs);

    memset(pstExpFuncs, 0, sizeof(AF_DRIVER_FUNC_S));

    pstExpFuncs->pfnGetZoomCode    = EXTDEV_GetZoomCode;
    pstExpFuncs->pfnSetZoomCode    = EXTDEV_SetZoomCode;
    pstExpFuncs->pfnGetFocusCode   = EXTDEV_GetFocusCode;
    pstExpFuncs->pfnSetFocusCode   = EXTDEV_SetFocusCode;
    pstExpFuncs->pfnGetMotorCode   = EXTDEV_IspGetMotorCode;
    pstExpFuncs->pfnSetMotorCode   = EXTDEV_IspSetMotorCode;
    pstExpFuncs->pfnMotorReset     = EXTDEV_MotorReset;
    pstExpFuncs->pfnMotorBusyReq   = EXTDEV_MotorBusyReq;

    return EI_SUCCESS;
}
static EI_S32 EXTDEV_RegisterIspCallback(VISS_DEV Dev, ISP_DEV dev, ISP_3ALIB_S *pstAfLib)
{
    EI_S32 s32Ret;
    AF_DRIVER_REGISTER_S  stAfRegister;

    CAMERA_CHECK_POINTER(pstAfLib);

    SNS_SET_ISP_TO_VISS(dev, Dev);

    stAfRegister.AfDrvId = DEV_ID;

    s32Ret  = EXTDEV_InitAfExpFunction(&stAfRegister.stAfDrv);
    s32Ret |= EI_MI_AF_DriverRegCallBack(dev, pstAfLib, &stAfRegister);

    if (EI_SUCCESS != s32Ret) {
        EI_TRACE_ISP(EI_DBG_ERR, "%s register callback function to af lib failed!\n", DEV_NAME);
        return s32Ret;
    }

    return EI_SUCCESS;
}

static EI_S32 EXTDEV_UnregisterIspCallback(ISP_DEV dev, ISP_3ALIB_S *pstAfLib)
{
    EI_S32 s32Ret;

    CAMERA_CHECK_POINTER(pstAfLib);

    s32Ret = EI_MI_AF_DriverUnRegCallBack(dev, pstAfLib, DEV_ID);
    if (EI_SUCCESS != s32Ret) {
        EI_TRACE_ISP(EI_DBG_ERR, "%s unregister callback function to af lib failed!\n", DEV_NAME);
        return s32Ret;
    }

    return EI_SUCCESS;
}

EXT_DEV_OBJ_S stExtDevMs41929Obj = {
    .pfnRegisterVissCallback    = EXTDEV_RegisterVissCallback,
    .pfnUnRegisterVissCallback  = EXTDEV_UnregisterVissCallback,
    .pfnRegisterIspCallback    = EXTDEV_RegisterIspCallback,
    .pfnUnRegisterIspCallback  = EXTDEV_UnregisterIspCallback,
};

#ifdef __cplusplus
#if __cplusplus
}
#endif
#endif /* __cplusplus */

